#include "pwm.h"

void pwm_init(pwm_sel pwmid, u8 duty)
{
    u32 timer_sel = TIMER0_BASE_ADDR + ((pwmid / 4) << 12);
    u32 pwm_pin   = pwmid % 4;
    u32 VALUE0_0_level, VALUE0_0_count;
    u32 VALUE0_1_level, VALUE0_1_count;
    u32 VALUE1_0_level, VALUE1_0_count;
    u32 VALUE1_1_level, VALUE1_1_count;

    INTDEV_SET_CLK_RST(timer_sel,(INTDEV_RUN|INTDEV_IS_GROUP0|INTDEV_CLK_IS_CORECLK));

    TIMER_SET_OVERRIDE_GPIO(timer_sel, TIMER_P0_OVERRIDE_GPIO << (pwm_pin * 4));
    TIMER_SET_OUTPUT_EN(timer_sel, TIMER_P0_OUTPUT_ENABLE << (pwm_pin * 4));
    TIMER_SET_OUT_PORT(timer_sel, pwm_pin << (pwm_pin * 4));
    TIMER_SET_COMPARER_MODE(timer_sel, TIMER_COMPARER0_NOT_FORCE << (pwm_pin * 4));

    TIMER_SET_MAIN_CNT_BEGIN_VALUE0(timer_sel,TIMER_MAIN_CNT_COUNT_UP,0x00000000);
    TIMER_SET_MAIN_CNT_END_VALUE0(timer_sel,0x1000);
    TIMER_SET_MAIN_CNT_BEGIN_VALUE1(timer_sel,TIMER_MAIN_CNT_COUNT_DOWN,0x00008000);
    TIMER_SET_MAIN_CNT_END_VALUE1(timer_sel,0x00007000);
    TIMER_SET_CMD(timer_sel,TIMER_CMD_RESTART);

    if(duty == 0)
    {
        VALUE0_0_level = TIMER_COMPARER_OUTPUT0;
        VALUE0_1_level = TIMER_COMPARER_OUTPUT0;
        VALUE1_0_level = TIMER_COMPARER_OUTPUT0;
        VALUE1_1_level = TIMER_COMPARER_OUTPUT0;

        VALUE0_0_count = 0x0000;
        VALUE0_1_count = 0x1000;
        VALUE1_0_count = 0x7000;
        VALUE1_1_count = 0x8000;
    }
    else if(duty <= 50)
    {
        VALUE0_0_level = TIMER_COMPARER_OUTPUT1;
        VALUE0_1_level = TIMER_COMPARER_OUTPUT0;
        VALUE1_0_level = TIMER_COMPARER_OUTPUT0;
        VALUE1_1_level = TIMER_COMPARER_OUTPUT0;

        VALUE0_0_count = 0x0000;
        VALUE0_1_count = duty * 0x1000 / 50;
        VALUE1_0_count = 0x7000;
        VALUE1_1_count = 0x8000;

    }
    else if(duty < 100)
    {
        VALUE0_0_level = TIMER_COMPARER_OUTPUT1;
        VALUE0_1_level = TIMER_COMPARER_OUTPUT1;
        VALUE1_0_level = TIMER_COMPARER_OUTPUT1;
        VALUE1_1_level = TIMER_COMPARER_OUTPUT0;

        VALUE0_0_count = 0x0000;
        VALUE0_1_count = 0x1000;
        VALUE1_0_count = 0x7000;
        VALUE1_1_count = 0x8000 - ((duty - 50) * 0x1000 / 50);
    }
    else
    {
        if(duty > 100)
        {
            duty = 100;
        }
        VALUE0_0_level = TIMER_COMPARER_OUTPUT1;
        VALUE0_1_level = TIMER_COMPARER_OUTPUT1;
        VALUE1_0_level = TIMER_COMPARER_OUTPUT1;
        VALUE1_1_level = TIMER_COMPARER_OUTPUT1;

        VALUE0_0_count = 0x0000;
        VALUE0_1_count = 0x1000;
        VALUE1_0_count = 0x7000;
        VALUE1_1_count = 0x8000;

    }

    switch(pwm_pin)
    {
    case 0:
        TIMER_SET_COMPARER0_VALUE0_0(timer_sel,VALUE0_0_level,VALUE0_0_count);
        TIMER_SET_COMPARER0_VALUE0_1(timer_sel,VALUE0_1_level,VALUE0_1_count);
        TIMER_SET_COMPARER0_VALUE1_0(timer_sel,VALUE1_0_level,VALUE1_0_count);
        TIMER_SET_COMPARER0_VALUE1_1(timer_sel,VALUE1_1_level,VALUE1_1_count);
        break;
    case 1:
        TIMER_SET_COMPARER1_VALUE0_0(timer_sel,VALUE0_0_level,VALUE0_0_count);
        TIMER_SET_COMPARER1_VALUE0_1(timer_sel,VALUE0_1_level,VALUE0_1_count);
        TIMER_SET_COMPARER1_VALUE1_0(timer_sel,VALUE1_0_level,VALUE1_0_count);
        TIMER_SET_COMPARER1_VALUE1_1(timer_sel,VALUE1_1_level,VALUE1_1_count);
        break;
    case 2:
        TIMER_SET_COMPARER2_VALUE0_0(timer_sel,VALUE0_0_level,VALUE0_0_count);
        TIMER_SET_COMPARER2_VALUE0_1(timer_sel,VALUE0_1_level,VALUE0_1_count);
        TIMER_SET_COMPARER2_VALUE1_0(timer_sel,VALUE1_0_level,VALUE1_0_count);
        TIMER_SET_COMPARER2_VALUE1_1(timer_sel,VALUE1_1_level,VALUE1_1_count);
        break;
    case 3:
        TIMER_SET_COMPARER3_VALUE0_0(timer_sel,VALUE0_0_level,VALUE0_0_count);
        TIMER_SET_COMPARER3_VALUE0_1(timer_sel,VALUE0_1_level,VALUE0_1_count);
        TIMER_SET_COMPARER3_VALUE1_0(timer_sel,VALUE1_0_level,VALUE1_0_count);
        TIMER_SET_COMPARER3_VALUE1_1(timer_sel,VALUE1_1_level,VALUE1_1_count);
        break;
    }

    TIMER_SET_CMD(timer_sel,TIMER_CMD_RUN);
}
